#include "CollisionTest.h"
#include "TestManager.h"

#include <cassert>
#include <cmath>

using namespace phynet;

const Real CollisionTest::PARTICLE_STARTING_SPEED = 20.0f;
const Real CollisionTest::PARTICLE_STARTING_ANGLE_GRAD = 180.0f;
const Real CollisionTest::PARTICLE_STARTING_DAMPING = 0.990f;

CollisionTest::CollisionTest(TestManager* testManager)
	: Test(testManager)
{
	Init();
}

CollisionTest::~CollisionTest()
{
}

void CollisionTest::Init()
{
	using namespace math;
	using namespace phy;
	m_engine.Init();

	m_nextParticleSpeed = PARTICLE_STARTING_SPEED;
	m_nextParticleAngleGrad = PARTICLE_STARTING_ANGLE_GRAD;
	m_nextParticleDamping = PARTICLE_STARTING_DAMPING;

	const Real PARTICLE_MASS[NUM_PARTICLES] = {1.0f, 0.1f, 0.7f, 3.0f};
	const Real PARTICLE_SIZE[NUM_PARTICLES] = {0.4f, 0.1f, 0.3f, 0.7f};
	const Real PARTICLE_X_POSITION[NUM_PARTICLES] = { 10.0f,  -5.0f, -10.0f, -10.0f};
	const Real PARTICLE_Y_POSITION[NUM_PARTICLES] = {  5.0f,   5.0f,  10.0f,   0.0f};

	for(size_t i=0; i< NUM_PARTICLES; ++i)
	{
		m_particle[i] = Particle2D(
			PARTICLE_MASS[i],
			PARTICLE_SIZE[i],
			PARTICLE_STARTING_DAMPING, 
			1.0f, 
			Vector2(PARTICLE_X_POSITION[i], PARTICLE_Y_POSITION[i]),
			Vector2::ZERO());
	}
	
	m_engine.GetWorld().AddParticles(m_particle, NUM_PARTICLES);

	Polygon2::Points points;
	points.push_back(Vector2(-15.0f, -5.0f));
	points.push_back(Vector2(15.0f, -5.0f));
	points.push_back(Vector2(15.0f, 15.0f));
	points.push_back(Vector2(-15.0f, 15.0f));
	Boundary2D theBoundary;
	theBoundary.m_poly.SetPoints(points);
	theBoundary.m_elasticity = 1.0f;
	m_engine.SetWorldBoundary(theBoundary);
}

void CollisionTest::Shoot()
{
	if(!math::AreEqual(m_particle[0].GetVelocity().SquareMagnitude(), 0.0f))
		return;

	const Real angleRad = m_nextParticleAngleGrad * 3.1415926f / 180.0f;
	math::Vector2 impulse(m_nextParticleSpeed*cosf(angleRad), m_nextParticleSpeed*sinf(angleRad));
	impulse *= m_particle[0].GetMass();
	m_particle[0].AddImpulse(impulse);
}

void CollisionTest::Render(Renderer& renderer) const
{
	typedef math::Vector2 Vector2;
	typedef phy::ParticleWorld2D::Particles Particles;

	const Real speed = m_testManager->GetSlowMotion();

	// instruction
	char buffer[100];
	sprintf(buffer, "Ciao!! e' passato: %.2f  velocita' %.2fX", m_engine.GetWorld().GetTime(), speed);
	renderer.RenderTextLine(buffer);
	sprintf(buffer, "Speed %.2f (-V/+B)", m_nextParticleSpeed);
	renderer.RenderTextLine(buffer);
	sprintf(buffer, "Direction %.2f (-C/+X)", m_nextParticleAngleGrad);
	renderer.RenderTextLine(buffer);
	sprintf(buffer, "Damping %.3f (-K/+L)", m_nextParticleDamping);
	renderer.RenderTextLine(buffer);

	// instruction
	sprintf(buffer, "");
	renderer.RenderTextLine(buffer);
	sprintf(buffer, "Particle-Particle Collisions %s - (toggle Y)", (m_engine.IsParticleCollisionsEnabled() ? "enabled" : "disabled"));
	renderer.RenderTextLine(buffer);
	sprintf(buffer, "Particle-World Collisions %s - (toggle U)", (m_engine.IsWorldCollisionsEnabled() ? "enabled" : "disabled"));
	renderer.RenderTextLine(buffer);
	sprintf(buffer, "");
	renderer.RenderTextLine(buffer);

	// cannon
	if(math::AreEqual(m_particle[0].GetVelocity().SquareMagnitude(), 0.0f))
	{
		const Real angleRad = m_nextParticleAngleGrad * 3.1415926f / 180.0f;
		Vector2 direct(cosf(angleRad), sinf(angleRad));
		renderer.RenderLine(m_particle[0].GetPosition(), m_particle[0].GetPosition() + direct * m_nextParticleSpeed/4.0f);
	}

	// world
	const Particles& particles = m_engine.GetWorld().GetParticles();
	for (Particles::const_iterator i = particles.begin(); i != particles.end(); ++i)
		renderer.RenderCircle((*i)->GetPosition(), (*i)->GetRadius(), 12);

	//outer boundary
	const math::Polygon2::Points& points = m_engine.GetWorldBoundary().m_poly.GetPoints();
	const size_t numPoints = points.size();
	for (size_t j=0; j<numPoints; ++j)
		renderer.RenderLine(points[j], points[(j+1)%numPoints]);		
}

void CollisionTest::Update(float i_elapsedTime)
{
	m_particle[0].SetDamping(m_nextParticleDamping);
	m_engine.Update(i_elapsedTime);
}

void CollisionTest::OnKeyPressed(int keyCode)
{
	if (keyCode == sf::Key::S)
	{
		Shoot();
	}
	else if (keyCode == sf::Key::X)
	{
		m_nextParticleAngleGrad += 1.0f;
	}
	else if (keyCode == sf::Key::C)
	{
		m_nextParticleAngleGrad -= 1.0f;
	}
	else if (keyCode == sf::Key::B)
	{
		if(m_nextParticleSpeed < 30.0f)
			m_nextParticleSpeed += 1.0f;
	}
	else if (keyCode == sf::Key::V)
	{
		if(m_nextParticleSpeed >= 2.0f)
			m_nextParticleSpeed -= 1.0f;
	}
	else if (keyCode == sf::Key::L)
	{
		if(math::LessThan(m_nextParticleDamping,1.0f))
			m_nextParticleDamping += 0.001f;
	}
	else if (keyCode == sf::Key::K)
	{
		m_nextParticleDamping -= 0.001f;
	}
	else if (keyCode == sf::Key::Y)
	{
		m_engine.EnableParticleCollisions(!m_engine.IsParticleCollisionsEnabled());
	}
	else if (keyCode == sf::Key::U)
	{
		m_engine.EnableWorldCollisions(!m_engine.IsWorldCollisionsEnabled());
	}

}
